/**
 * @file   method.h
 * @author xingyifan <xingyifan@xingyifan-VirtualBox>
 * @date   Tue Jul 13 14:40:20 2021
 * 
 * @brief  Numerical Algebra
 * 
 * 
 */

#include "Matrix.h"

using namespace Algebra;

template <typename T>
void LU(Matrix<T> &A) {
    int n = A.get_row();
    for (int i=0;i<n;i++) {
	double key = A[i][i];
	for (int j=i+1;j<n;j++) {
	    A[j][i] = A[j][i] / key;
	    for (int k=i+1;k<n;k++) {
		A[j][k] -= A[i][k] * A[j][i];
	    }
	}
    }
}

// LL
template <typename T>
void cholesky(Matrix<T> &A) {
    int n = A.get_row();
    double sum = 0;
    for (int i=0;i<n;i++) {
	sum = 0;
	for (int p=0;p<i;p++) {
	    sum += A[i][p] * A[i][p];
	}
	A[i][i] = sqrt(A[i][i] - sum);
	for (int j=i+1;j<n;j++) {
	    sum = 0;
	    for (int k=0;k<i;k++) {
		sum += A[j][k] * A[i][k];
	    }
	    A[j][i] = (A[j][i] - sum) / A[i][i];
	    A[i][j] = A[j][i];
	}
    }
}

// U
template <typename T>
Vector<T> upper_tr(Matrix<T> &A, Vector<T> &b) {
    int n = b.get_len();
    Vector<T> x(n);
    double sum;
    for (int i=n-1;i>=0;i--) {
	sum = 0;
	for (int j=i+1;j<n;j++) {
	    sum += A[i][j] * x[j];
	}
	x[i] = (b[i] - sum) / A[i][i];
    }
    return x;
}

// L
template <typename T>
Vector<T> lower_tr(Matrix<T> &A, Vector<T> &b) {
    int n = b.get_len();
    Vector<T> x(n);
    double sum;
    x[0] = b[0];
    for (int i=1;i<n;i++) {
	sum = 0;
	for (int j=0;j<i;j++) {
	    sum += A[i][j] * x[j];
	}
	x[i] = b[i] - sum;
    }
    return x;
}

template <typename T>
Vector<T> columnLU(Matrix<T> &A, Vector<T> &b) {
    int n = A.get_row();
    Vector<T> x(n);
    double max, tmp;
    int index;
    for (int i=0;i<n;i++) {
	max = 0;
	index = i;
	for (int p=i;p<n;p++) {
	    tmp = fabs(A[p][i]);
	    if (tmp>max) {
		max = tmp;
		index = p;
	    }
	}
	if (max==0) {
	    cout << "Singular Matrix!" << endl;
	    return x;
	}
	for (int q=0;q<n;q++) {
	    tmp = A[i][q];
	    A[i][q] = A[index][q];
	    A[index][q] = tmp;
	}
	tmp = b[i];
	b[i] = b[index];
	b[index] = tmp;
	for (int j=i+1;j<n;j++) {
	    A[j][i] = A[j][i] / A[i][i];
	    for (int k=i+1;k<n;k++) {
		A[j][k] = A[j][k] - A[i][k] * A[j][i];
	    }
	}
    }
    x = lower_tr(A, b); 
    x = upper_tr(A, x);
    
    return x;
}

template <typename T>
Vector<T> rowLU(Matrix<T> &A, Vector<T> &b) {
    int n = A.get_row();
    Vector<T> x(n);
    double max, tmp;
    int index;
    for (int i=0;i<n;i++) {
	max = 0;
	index = i;
	for (int p=i;p<n;p++) {
	    tmp = fabs(A[i][p]);
	    if (tmp>max) {
		max = tmp;
		index = p;
	    }
	}
	if (max==0) {
	    cout << "Singular Matrix!";
	    return x;
	}
	for (int q=i;q<n;q++) {
	    tmp = A[q][i];
	    A[q][i] = A[q][index];
	    A[q][index] = tmp;
	}
	for (int j=i+1;j<n;j++) {
	    A[j][i] /= A[i][i];
	    for (int k=i+1;k<n;k++) {
		A[j][k] -= A[i][k] * A[j][i];
	    }
	}
    }
    x = lower_tr(A, b);
    x = upper_tr(A, x);
    
    return x;
}

